/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation of MaCI interface encapsulation for J2B2.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "J2B2ClientInterface.hpp"
#include "owndebug.h"

//*****************************************************************************
//*****************************************************************************
using namespace MaCI;
//*****************************************************************************

#define INIT_INTERFACE(to, iftype, ifname, ifid) {                            \
        const MaCICtrl::SMaCISL sl##to(iMaCIGroup, (ifname), (ifid));         \
        const std::string slstr##to = sl##to.ToMaCISLString();                \
        dPrint(3,"Initializing '%s'", slstr##to.c_str());                     \
        (to) = new iftype (iGIMIPtr, -1);                                     \
        if ((to)->SetDefaultTarget(sl##to, aServices)) {                      \
          if ((to)->Open() == KMaCIOK) {                                      \
          dPrintLCGreen(2,"Succesfully Initialized '%s'!", slstr##to.c_str());\
          iAllClients.push_back((to));                                        \
          } else {                                                            \
            result = false;                                                   \
          }                                                                   \
        } else result = false;                                                \
        if (!result) {                                                        \
          dPrintLCRed(2,"Failed to Initialize '%s'!", slstr##to.c_str());     \
        }                                                                     \
      }
//*****************************************************************************

CJ2B2ClientInterface::CJ2B2ClientInterface(gimi::GIMI *aGIMIptr)
  : iImageCameraFront(NULL),
    iImageCameraExtra(NULL),
    iServoCtrl(NULL),
    iMotionCtrl(NULL),
    iPositionOdometry(NULL),
    iBehaviourCtrl(NULL),
    iRangingLaser(NULL),
    iRangingBumpers(NULL),
    iIOBoardAD5414(NULL),
    iIOBoardESC(NULL),
    iTextToSpeech(NULL),
    iGIMIPtr(aGIMIptr),
    iEnabledInterfaces(KEnabledInterfaceNone),
    iMaCIGroup(),
    iAllClients()
{
  dPrint(2,"Constructed");
}
//*****************************************************************************

CJ2B2ClientInterface::~CJ2B2ClientInterface()
{
  for (EACH_IN_i(iAllClients)) {
    MaCI::CMaCI *instance = *i;
    if (instance) delete instance;
  }
  iAllClients.clear();
  dPrint(2,"Destructed.");
}
//*****************************************************************************

bool CJ2B2ClientInterface::Open(const std::string &aMaCIGroup, 
                                const int aEnabledInterfaces)
{
  bool result = false;

  dPrint(2, "Interface Opening...");
  iMaCIGroup = aMaCIGroup; // Store MaCIGroup
  iEnabledInterfaces = (EEnabledInterface)aEnabledInterfaces;


  // MaCICtrl client
  MaCICtrl::CMaCICtrlClient ctrlclient(iGIMIPtr);

  // Open MaCICtrl client.
  if (ctrlclient.Open() == KMaCIOK) {

    // Get all MaCI services from local GIMnet.
    gimi::t_serviceList macictrlservicelist;
    if (ctrlclient.DiscoverMaCICtrlServices(macictrlservicelist) == KMaCIOK) {
      
      // Request information from all MaCI nodes on the Net.
      MaCICtrl::TServiceEntryArray services;
      if (ctrlclient.GetServiceArray(services, macictrlservicelist) == MaCI::KMaCIOK) {
        
        // Call Init with service list.
        result = InitializeServices(services);
        
        if (result) {
          dPrint(2,"Service Initialization completed OK - %d services initialized",
                 iAllClients.size());
          result = true;

        } else {
          dPrint(1,"Service Initialization failed! J2B2ClientInterface is NOT succesfully opened!");

        }

      } else {
        dPrint(1,"Receiving Services List Failed! J2B2ClientInterface is NOT succesfully opened!");
        
      } 
    }
  } 

  return result;
}
//*****************************************************************************

bool CJ2B2ClientInterface::Close(void)
{
  dPrint(2, "Interface Closing...");
  for (EACH_IN_i(iAllClients)) {
    MaCI::CMaCI *instance = *i;
    if (instance) instance->Close();
  }
  return true;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//********** Private Functions ************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

bool 
CJ2B2ClientInterface::InitializeServices(const MaCI::MaCICtrl::TServiceEntryArray &aServices)
{
  bool result = true;

  // 1. Camera services (both images + ptu)
  result &= InitializeCamera(aServices);
  
  // 2. Control services (speedctrl, behaviour)
  result &= InitializeControls(aServices);

  // 3. Information Services (odometry, slam)
  result &= InitializeInfos(aServices);

  // 4. Sensor services (laser, bumpers, IO)
  result &= InitializeSensors(aServices);

  return result;
}
//*****************************************************************************

bool 
CJ2B2ClientInterface::InitializeCamera(const MaCI::MaCICtrl::TServiceEntryArray &aServices)
{
  using namespace MaCI::Image;
  bool result = true;

  // Is camera enabled?
  if (iEnabledInterfaces & KEnabledInterfaceImageCameraFront) {
    INIT_INTERFACE(iImageCameraFront, Image::CImageClient, 
                   "MaCI_Image", "CameraFront");

  }

  // Is PTU enabled?
  if (result && iEnabledInterfaces & KEnabledInterfaceServoCtrl) {
    INIT_INTERFACE(iServoCtrl, JointGroupCtrl::CJointGroupCtrlClient, 
                   "MaCI_JointGroupCtrl", "ServoCtrl"); 

  }

  // Is Second Camera enabled?
  if (iEnabledInterfaces & KEnabledInterfaceImageCameraExtra) {
    INIT_INTERFACE(iImageCameraExtra, Image::CImageClient, 
                   "MaCI_Image", "ExtraCam");

  }

  
  
  return result;
}
//*****************************************************************************

bool 
CJ2B2ClientInterface::InitializeControls(const MaCI::MaCICtrl::TServiceEntryArray &aServices)
{
  bool result = true;
  
  // SpeedCtrl enabled?
  if (iEnabledInterfaces & KEnabledInterfaceSpeedCtrl) {
    INIT_INTERFACE(iMotionCtrl, SpeedCtrl::CSpeedCtrlClient, 
                   "MaCI_SpeedCtrl", "Drive2D");

  }
    
  // Behaviours enabled?
  if (result && iEnabledInterfaces & KEnabledInterfaceBehaviourCtrl) {
    INIT_INTERFACE(iBehaviourCtrl, Behaviour::CBehaviourClient, 
                   "MaCI_Behaviour", "AvoidHit");

  }
  
  return result;
}
//*****************************************************************************

bool 
CJ2B2ClientInterface::InitializeInfos(const MaCI::MaCICtrl::TServiceEntryArray &aServices)
{
  bool result = true;  

  // Position::Odometry enabled?
  if (iEnabledInterfaces & KEnabledInterfacePositionOdometry) {
    INIT_INTERFACE(iPositionOdometry, Position::CPositionClient, 
                   "MaCI_Position", "Motion");

  }

  // Text::TTS enabled?
  if (result && iEnabledInterfaces & KEnabledInterfaceTextToSpeech) {
    INIT_INTERFACE(iTextToSpeech, Text::CTextClient, 
                   "MaCI_Text", "TTS");

  }

  
  return result;
}
//*****************************************************************************

bool
CJ2B2ClientInterface::InitializeSensors(const MaCI::MaCICtrl::TServiceEntryArray &aServices)
{
  bool result = true;
  
  // Ranging::Laser enabled?
  if (iEnabledInterfaces & KEnabledInterfaceRangingLaser) {
    INIT_INTERFACE(iRangingLaser, Ranging::CRangingClient, 
                   "MaCI_Ranging", "Laser");

  }


  // Ranging::Bumpers enabled?
  if (result && iEnabledInterfaces & KEnabledInterfaceRangingBumpers) {
    INIT_INTERFACE(iRangingBumpers, Ranging::CRangingClient, 
                   "MaCI_Ranging", "Bumpers");

  }


  // IOBoard - AD5414 enabled?
  if (result && iEnabledInterfaces & KEnabledInterfaceIOBoardAD5414) {
    INIT_INTERFACE(iIOBoardAD5414, IO::CIOClient, 
                   "MaCI_IO", "AD5414");

  }


  // IOBoard - ESC enabled?
  if (result && iEnabledInterfaces & KEnabledInterfaceIOBoardESC) {
    INIT_INTERFACE(iIOBoardESC, IO::CIOClient, 
                   "MaCI_IO", "Motion");

  }
  


  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
